/*
 * @Author: your name
 * @Date: 2021-06-04 22:40:32
 * @LastEditTime: 2021-08-01 19:25:27
 * @LastEditors: Please set LastEditors
 * @Description: In User Settings Edit
 * @FilePath: \TEST\Core\mycode\control.h
 */

#include "pid.h"
#include "encoder.h"
#include "servo.h"
#include"openmv.h"
#define TEST 0
#define KAIHUAN 1
#define STOP_THE_CAR 2
#define PI (3.14159265358979323846)
#define PI_DIV_180 (0.017453292519943296)//π/180
#define DegToRad(x) ((x)*PI_DIV_180)//角度转换为弧度
#define RadToDeg(x) ((x)/PI_DIV_180)


//此结构体中记录目标的角速度和线速度，是提供给策略输出的接口，当然要修改里面的值需要include“control.h”

/**
 * @description:用于速度均值滤波 
 * @param {*}
 * @return {*}
 */
typedef struct 
{
    float wheel_1[5];
    int times;
    float speed;
}LAST_speeds;

/**
 * @description:用来存放PID的参数以及各PID要达到的目标量 
 * @param {*}
 * @return {*}
 */
typedef struct 
{
    ST_PID motor1_d;
    ST_PID motor1_v;
    ST_PID motor2_d;
    ST_PID motor2_v;
    ST_PID target_v;
    float target_speed;
    float target_distence;
    float fangkuai_target_speed;
    float target_speed_2;
}PID_S;

typedef struct 
{
    int M1_front;
    int M1_back;
    int M2_front;
    int M2_back;
    int M3_front;
    int M3_back;
    int M4_front;
    int M4_back;
}SIQU;

/**
 * @description: 用来存放接收到的数据的缓冲区
 * @param {*}
 * @return {*}
 */
typedef struct
{
    uint8_t a_buffer[1];
    uint8_t data_buffer[50];
    int counter;//计数收到了几个数据
    int finished_data;//结束收集数据的标志
}CHANGE_Data_Rx;
extern CHANGE_Data_Rx change_data_rx;

/**
 * @description: 用来保存接收到的数据，
 * @param {*}
 * @return {*}
 */
typedef struct
{
    ONE_class_data class[3];
}RECEIVED_data;
extern RECEIVED_data received_data;//接收到的数据的存放区
typedef struct 
{
    float speed_now[4];
    float distence_now[4];
    float target_positon;
    float target_v;
}CONTROL_data;


typedef struct 
{
    float last_x;
    float x_now;
    float v;
}TARGET_data;

#define HHTARGET_SPEED  1000
#define HHTARGET_DISTANCE  1560
#define HHTARGET_FANGKUAI_SPEED 0
#define HHPD1  0.8
#define HHID1  0.001
#define HHDD1  0
#define HHPV1  15
#define HHIV1  0
#define HHDV1  0
#define HHPD2  0.8
#define HHID2  0.001
#define HHDD2  0
#define HHPV2  15
#define HHIV2  0
#define HHDV2  0
#define HHPT  0
#define HHIT  0
#define HHDT  0
#define fpSumE_max_d 60000  //位置环积分输出上限
#define fpSumE_max_v 0//速度环积分输出上限
#define fpSumE_max_t 30 //最外环积分输出上限制

void control_data_process(void);
extern RECEIVED_data received_data;
extern CONTROL_data control_data;
extern test_switch;
extern derta_pwm[4];
extern float i_test_pid_line2;
extern PID_S pid_s;


//大电机的参数
// #define HHPD1  0.8
// #define HHID1  0.001
// #define HHDD1  0
// #define HHPV1  15
// #define HHIV1  0
// #define HHDV1  0
// #define HHPT1  0
// #define HHIT1  0
// #define HHDT1  0
// #define HHPD2  0.8
// #define HHID2  0.001
// #define HHDD2  0
// #define HHPV2  15
// #define HHIV2  0
// #define HHDV2  0
// #define HHPT2  0
// #define HHIT2  0
// #define HHDT2  0
